#line 1 "D:/Archivio/mikroC/tric-motors/cottero.c"
#line 26 "D:/Archivio/mikroC/tric-motors/cottero.c"
unsigned short val_motors[4];
unsigned short acceleration[6];
unsigned short tdown;

char data_, error;
#line 35 "D:/Archivio/mikroC/tric-motors/cottero.c"
void Soft_UART_Write_Text(char* testo) {
 char cnt;
 for (cnt=0; cnt<strlen(testo); cnt++) {
 Soft_UART_Write(testo[cnt]);
 }
}
#line 44 "D:/Archivio/mikroC/tric-motors/cottero.c"
void ADXL345_Wr(unsigned short address, unsigned short in) {
 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(address);
 I2C1_Wr(in);
 I2C1_Stop();
}
#line 54 "D:/Archivio/mikroC/tric-motors/cottero.c"
unsigned short ADXL345_Rd(unsigned short address) {
 unsigned short out;
 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(address);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr(0x3B);
 out = I2C1_Rd(0);
 I2C1_Stop();
 return out;
}
#line 71 "D:/Archivio/mikroC/tric-motors/cottero.c"
void ADXL345_getAcc(int *acc) {
 unsigned short cnt;
 unsigned short acc_tmp[6];
 char z_string[8];

 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(0x32);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr(0x3B);
 for (cnt=0; cnt<6; cnt++) {
 char ack;
 ack = (cnt<5);
 acc_tmp[cnt] = I2C1_Rd(ack);
 }
 I2C1_Stop();

 acc[0] = acc_tmp[1];
 acc[0] = acc[0] << 8;
 acc[0] |= acc_tmp[0];
 acc[1] = acc_tmp[3];
 acc[1] = acc[1] << 8;
 acc[1] |= acc_tmp[2];
 acc[2] = acc_tmp[5];
 acc[2] = acc[2] << 8;
 acc[2] |= acc_tmp[4];
}
#line 102 "D:/Archivio/mikroC/tric-motors/cottero.c"
void dataArrayInit(int *fifox, int *fifoy, int *fifoz) {
 unsigned short cnt;
 int acc_tmp[3];
 for (cnt=0; cnt< 16 ; cnt++) {
 ADXL345_getAcc(&acc_tmp);
 fifox[cnt] = acc_tmp[0];
 fifoy[cnt] = acc_tmp[1];
 fifoz[cnt] = acc_tmp[2];
 }
}
#line 115 "D:/Archivio/mikroC/tric-motors/cottero.c"
void dataArrayUpdate(int *fifox, int *fifoy, int *fifoz) {
 static unsigned short table_pointer;
 int acc_tmp[3];
 if (table_pointer < 0 || table_pointer >=  16 ) table_pointer = 0;
 ADXL345_getAcc(&acc_tmp);
 fifox[table_pointer] = acc_tmp[0];
 fifoy[table_pointer] = acc_tmp[1];
 fifoz[table_pointer] = acc_tmp[2];
 table_pointer++;
}
#line 128 "D:/Archivio/mikroC/tric-motors/cottero.c"
int dataArrayFilter(int *fifo) {
 unsigned short cnt;
 long value = 0;
 for (cnt=0; cnt< 16 ; cnt++) {
 value += fifo[cnt];
 }
 return (int)(value/ 16 );
}

void Motor_ctrl(){
 unsigned short cnt, cnt1, max;
 cnt1 = max = 0;

 if(tdown==20) {
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  =  PORTB.B5  =  0 ;
 }
 if(tdown==21) {
 tdown = 0;
 T0CON.TMR0ON=0;
 for(cnt=0; cnt<4; cnt++) {
 max = (val_motors[cnt]>=max) ? val_motors[cnt] : max;
 }
 while(cnt1<=max) {
 if(cnt1==val_motors[0])  PORTB.B2  = ! 0 ;
 if(cnt1==val_motors[1])  PORTB.B3  = ! 0 ;
 if(cnt1==val_motors[2])  PORTB.B4  = ! 0 ;
 if(cnt1==val_motors[3])  PORTB.B5  = ! 0 ;
 Delay_us(8);
 cnt1++;
 }
 cnt1=max=0;
 TMR0L=70;
 T0CON.TMR0ON=1;
 }
}
#line 167 "D:/Archivio/mikroC/tric-motors/cottero.c"
void ESC_startup(){
 unsigned short cnt=0;
 while(cnt<200) {
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  =  0 ;
 Delay_ms(1);
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  = ! 0 ;
 Delay_ms(20);
 cnt++;
 }
 T0CON.TMR0ON=1;
}
#line 183 "D:/Archivio/mikroC/tric-motors/cottero.c"
void ESC_config(){
 unsigned short cnt=0;
 while(cnt<150){
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  =  0 ;
 Delay_ms(2);
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  = ! 0 ;
 Delay_ms(20);
 cnt++;
 }
 cnt = 0;
 while(cnt<150){
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  =  0 ;
 Delay_ms(1);
  PORTB.B2  =  PORTB.B3  =  PORTB.B4  = ! 0 ;
 Delay_ms(20);
 cnt++;
 }
 Delay_ms(2000);
}

void main() {

 int acc_now[3];
 int acc_x[ 16 ];
 int acc_y[ 16 ];
 int acc_z[ 16 ];

 char x_string[8];
 char y_string[8];
 char z_string[8];


  TRISC.B2  = 0;
  TRISB.B2  = 0;
  TRISB.B3  = 0;
  TRISB.B4  = 0;
  TRISB.B5  = 0;


 ADCON1 = 0x0F;
 CMCON = 0xCF;


 Soft_UART_Init(&PORTB, 6, 7, 38400, 0);
 I2C1_Init(400000);

 Delay_ms(2000);


 T0CON.T0PS0=1;
 T0CON.T0PS1=0;
 T0CON.T0PS2=1;
 T0CON.PSA=0;
 T0CON.T0CS=0;
 T0CON.T08BIT=1;
 INTCON.TMR0IE=1;
 INTCON.PEIE=1;
 INTCON.GIE=1;
 T0CON.TMR0ON=0;
 TMR0L=68;
 TMR0H =0;
#line 261 "D:/Archivio/mikroC/tric-motors/cottero.c"
 dataArrayInit(&acc_x, &acc_y, &acc_z);

  PORTB.B2  =  PORTB.B3  =  PORTB.B4  =  PORTB.B5  = ! 0 ;
 tdown=0;
 val_motors[0]=0;
 val_motors[1]=0;
 val_motors[2]=0;
 val_motors[3]=0;

 ESC_startup();
 val_motors[1]=10;
 val_motors[2]=30;
 val_motors[3]=60;


 while (1) {
 dataArrayUpdate(&acc_x, &acc_y, &acc_z);

 dataArrayFilter(&acc_x);
 dataArrayFilter(&acc_y);
 dataArrayFilter(&acc_z);
#line 294 "D:/Archivio/mikroC/tric-motors/cottero.c"
 do
 data_ = Soft_UART_Read(&error);
 while (error);
 val_motors[0]=data_;

 }
}


void interrupt() {



 if( INTCON.TMR0IF ){
 TMR0L=70;
 INTCON.TMR0IF=0;
 tdown++;
 Motor_ctrl();
 }



 }
